//
// Ephi - simulation of magnetic fields and particles
// Copyright (C) 2007, 2008 Indrek Mandre <indrek(at)mare.ee>
// For more information please see http://www.mare.ee/indrek/
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License along
// with this program; if not, write to the Free Software Foundation, Inc.,
// 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
//

#include <assert.h>

#include "octree.hpp"
#include "tricubic.hpp"

OctreeBuilder::OctreeBuilder (const vect3d& c1, const vect3d& c2,
    size_t nScalars, size_t nVectors, size_t limitMemoryMB, prec_t smallestResolution)
{
  this->c1 = c1;
  this->c2 = c2;
  this->nScalars = nScalars;
  this->nVectors = nVectors;
  this->nValues = nScalars + 3 * nVectors;
  this->limitMemoryMB = limitMemoryMB;
  this->smallestResolution = smallestResolution;
  this->limitRadius = 0;
}

void OctreeBuilder::addLimit (const vect3d& n)
{
  limits.push_back (n);
}

void OctreeBuilder::setLimitRadius (prec_t limitRadius)
{
  this->limitRadius = limitRadius;
}

OctreeBuilder::~OctreeBuilder ()
{
}

void OctreeBuilder::build (Octree& tree)
{
  assert (c1.x < c2.x && c1.y < c2.y && c1.z < c2.z);
  assert (isInside (c1, c2));

  cur.clear();
  values_map_calc.clear();
  values_map.clear();

  tree.clear();
  tree.nScalars = nScalars;
  tree.nVectors = nVectors;
  tree.nValues = nValues;
  tree.corner = c1;
  tree.box = c2 - c1;
  tree.hbox = 0.5 * tree.box;
  tree.rbox = tree.box.recip();

  tree.xc = (size_t)(tree.box.x / smallestResolution);
  tree.yc = (size_t)(tree.box.y / smallestResolution);
  tree.zc = (size_t)(tree.box.z / smallestResolution);

  if ( !tree.xc ) tree.xc++;
  if ( !tree.yc ) tree.yc++;
  if ( !tree.zc ) tree.zc++;

  tree.levels.push_back (Octree::level_t());
  tree.levels.back().bs = vect3d((c2.x - c1.x) / tree.xc, (c2.y - c1.y) / tree.yc, (c2.z - c1.z) / tree.zc);
  tree.levels.back().hbs = 0.5 * tree.levels.back().bs;
  tree.levels.back().rbs = tree.levels.back().bs.recip();

  size_t refuse_count = 0;

  cur.reserve (1000);
  timer t;

  for ( size_t xi = 0; xi < tree.xc; xi++ )
    {
      prec_t x = c1.x + xi * tree.box.x / tree.xc;
      prec_t xn = c1.x + (xi + 1) * tree.box.x / tree.xc;
      for ( size_t yi = 0; yi < tree.yc; yi++ )
        {
          prec_t y = c1.y + yi * tree.box.y / tree.yc;
          prec_t yn = c1.y + (yi + 1) * tree.box.y / tree.yc;
          for ( size_t zi = 0; zi < tree.zc; zi++ )
            {
              prec_t z = c1.z + zi * tree.box.z / tree.zc;
              prec_t zn = c1.z + (zi + 1) * tree.box.z / tree.zc;
              vect3d c1(x, y, z);
              vect3d c2(xn, yn, zn);

              if ( !isInside (c1, c2) || limitRadius != 0 && c1.magnitude() > limitRadius * limitRadius &&
                  c2.magnitude() > limitRadius * limitRadius )
                {
                  refuse_count++;
                  continue;
                }

              cur.push_back (o8entry_t());
              cur.back().cell = tree.a_cell.getz();
              cur.back().cell->index = zi + yi * tree.zc + xi * tree.yc * tree.zc;
              cur.back().c1 = c1;
              cur.back().c2 = c2;
            }
        }
    }

  t.report ("splitup");

  printf ("cur.size=%u\n", (unsigned int)cur.size());

  values_map_calc.reserve (8000);

  t.reset();

  vect3d box = tree.levels.back().bs;
  vect3d corners[8];

  for ( size_t i = 0; i < cur.size(); i++ )
    {
      o8entry_t& e = cur[i];
      corners[0] = vect3d (e.c1.x, e.c1.y, e.c1.z);
      corners[1] = vect3d (e.c2.x, e.c1.y, e.c1.z);
      corners[2] = vect3d (e.c1.x, e.c2.y, e.c1.z);
      corners[3] = vect3d (e.c2.x, e.c2.y, e.c1.z);
      corners[4] = vect3d (e.c1.x, e.c1.y, e.c2.z);
      corners[5] = vect3d (e.c2.x, e.c1.y, e.c2.z);
      corners[6] = vect3d (e.c1.x, e.c2.y, e.c2.z);
      corners[7] = vect3d (e.c2.x, e.c2.y, e.c2.z);

      for ( size_t j = 0; j < 8; j++ )
        {
          values_map_t::iterator fit = values_map.find (corners[j]);
          if ( fit == values_map.end() )
            {
              values_map_calc.push_back (&values_map[corners[j]]);
              values_map_calc.back()->pos = corners[j];
              e.vals[j] = values_map_calc.back();
            }
          else
            {
              e.vals[j] = &fit->second;
            }
        }
    }

  t.report ("corner");
  printf ("corners.size = %u\n", (unsigned int)values_map.size());
  t.reset();

  TaskManager tmgr;
  std::vector<fieldtask> ftasks;
  ftasks.resize (THREADS_CPUCOUNT);
  for ( size_t i = 0; i < ftasks.size(); i++ )
    {
      ftasks[i].part = i;
      ftasks[i].builder = this;
      ftasks[i].values_map_calc = &values_map_calc;
      tmgr.addTask (&ftasks[i]);
    }

  while ( tmgr.run() );

  t.report ("cornercalc");

  tree.hash.resize (3 * cur.size() / 2 + 17 - cur.size() % 17, 0);
  for ( size_t i = 0; i < cur.size(); i++ )
    {
      o8entry_t& e = cur[i];
      e.cell->interp = tree.a_interp.get();
      e.cell->interp->corner = e.c1;
      e.cell->interp->fact = tree.a_fact.getz(64 * nValues);

      size_t hi = e.cell->index % tree.hash.size();
      e.cell->next = tree.hash[hi];
      tree.hash[hi] = e.cell;
    }

  t.report ("hash");

  std::vector<celltask> ctasks;
  ctasks.resize (THREADS_CPUCOUNT);
  for ( size_t i = 0; i < ctasks.size(); i++ )
    {
      ctasks[i].part = i;
      ctasks[i].builder = this;
      ctasks[i].cells = &cur;
      ctasks[i].box = box;
      tmgr.addTask (&ctasks[i]);
    }
  while ( tmgr.run() );

  t.report ("treecalc");

#if 0
  for ( size_t i = 0; i < tree.hash.size(); i++ )
    {
      printf ("%zu:", i);
      Octree::cell_t *cell = tree.hash[i];
      while ( cell )
        {
          printf (" %p", cell);
          cell = cell->next;
        }
      printf ("\n");
    }
  exit(0);
#endif
  printf ("%u, refused=%u\n", (unsigned int)cur.size(), (unsigned int)refuse_count);

#if 0
  size_t level = 0;
  size_t n = 0;
  while (1)
    {
      tree.levels.push_back (Octree::level_t());
      tree.levels.back().bs = cur[0].c2 - cur[0].c1;
      tree.levels.back().hbs = 0.5 * tree.levels.back().bs;
      tree.levels.back().rbs = tree.levels.back().bs.recip();

      bool added_subs = false;
      for ( size_t i = 0; i < cur.size(); i++ )
        {
          o8entry_t& e = cur[i];

          assert (e.c1.x < e.c2.x && e.c1.y < e.c2.y && e.c1.z < e.c2.z);

          vect3d diagonal = e.c2 - e.c1;

          for ( size_t j = 0; j < symmetry.size(); j++ )
            {
              vect3d& n = symmetry[j];
              if ( e.c1 * n > 0 ) continue;
              if ( e.c2 * n > 0 ) continue;
              if ( vect3d (e.c1.x + diagonal.x, e.c1.y, e.c1.z) * n > 0 ) continue;
              if ( vect3d (e.c1.x, e.c1.y + diagonal.y, e.c1.z) * n > 0 ) continue;
              if ( vect3d (e.c1.x, e.c1.y, e.c1.z + diagonal.z) * n > 0 ) continue;
              if ( vect3d (e.c2.x - diagonal.x, e.c2.y, e.c2.z) * n > 0 ) continue;
              if ( vect3d (e.c2.x, e.c2.y - diagonal.y, e.c2.z) * n > 0 ) continue;
              if ( vect3d (e.c2.x, e.c2.y, e.c2.z - diagonal.z) * n > 0 ) continue;

              e.cell->mirror = tree.a_mirror.get();
              e.cell->mirror->normal = n;

              Octree::cell_t *c = 0;
              vect3d f = 0.5 * (e.c1 + e.c2);
              f = f - 2 * (f * n) * n;
              for ( size_t k = 0; k < cur.size(); k++ )
                {
                  if ( (0.5 * (cur[k].c1 + cur[k].c2) - f).length() < 0.1 * smallestResolution )
                    {
                      c = cur[k].cell;
                      break;
                    }
                }
              assert (c);
              e.cell->mirror->cell = c;
              e.cell->mirror->corner = e.c1;
              break;
            }
          if ( e.cell->mirror )
              continue;

          if ( (e.c2 - e.c1).length() <= smallestResolution ) // XXX or field good enough for interpolation
            {
              e.cell->interp = tree.a_interp.get();
              e.cell->interp->fact = tree.a_fact.get (64 * nValues);
              e.cell->interp->corner = e.c1;

              getValues (0.5 * (e.c1 + e.c2), &values_cache[0]);
              for ( size_t j = 0; j < nValues; j++ )
                  e.cell->interp->fact[j] = values_cache[j];
              continue;
            }

          added_subs = true;
          vect3d add = 0.5 * (e.c2 - e.c1);
          e.cell->subcells = tree.a_subcell.getz();
          e.cell->subcells->middle = e.c1 + add;

          e.cell->subcells->sc[0] = tree.a_cell.getz();
          next.push_back (o8entry_t());
          next.back().cell = e.cell->subcells->sc[0];
          next.back().c1 = e.c1 + vect3d(0, 0, 0);
          next.back().c2 = e.c2 - vect3d(add.x, add.y, add.z);

          e.cell->subcells->sc[1] = tree.a_cell.getz();
          next.push_back (o8entry_t());
          next.back().cell = e.cell->subcells->sc[1];
          next.back().c1 = e.c1 + vect3d(0, add.y, 0);
          next.back().c2 = e.c2 - vect3d(add.x, 0, add.z);

          e.cell->subcells->sc[2] = tree.a_cell.getz();
          next.push_back (o8entry_t());
          next.back().cell = e.cell->subcells->sc[2];
          next.back().c1 = e.c1 + vect3d(0, 0, add.z);
          next.back().c2 = e.c2 - vect3d(add.x, add.y, 0);

          e.cell->subcells->sc[3] = tree.a_cell.getz();
          next.push_back (o8entry_t());
          next.back().cell = e.cell->subcells->sc[3];
          next.back().c1 = e.c1 + vect3d(0, add.y, add.z);
          next.back().c2 = e.c2 - vect3d(add.x, 0, 0);

          e.cell->subcells->sc[4] = tree.a_cell.getz();
          next.push_back (o8entry_t());
          next.back().cell = e.cell->subcells->sc[4];
          next.back().c1 = e.c1 + vect3d(add.x, 0, 0);
          next.back().c2 = e.c2 - vect3d(0, add.y, add.z);

          e.cell->subcells->sc[5] = tree.a_cell.getz();
          next.push_back (o8entry_t());
          next.back().cell = e.cell->subcells->sc[5];
          next.back().c1 = e.c1 + vect3d(add.x, add.y, 0);
          next.back().c2 = e.c2 - vect3d(0, 0, add.z);

          e.cell->subcells->sc[6] = tree.a_cell.getz();
          next.push_back (o8entry_t());
          next.back().cell = e.cell->subcells->sc[6];
          next.back().c1 = e.c1 + vect3d(add.x, 0, add.z);
          next.back().c2 = e.c2 - vect3d(0, add.y, 0);

          e.cell->subcells->sc[7] = tree.a_cell.getz();
          next.push_back (o8entry_t());
          next.back().cell = e.cell->subcells->sc[7];
          next.back().c1 = e.c1 + vect3d(add.x, add.y, add.z);
          next.back().c2 = e.c2 - vect3d(0, 0, 0);
          
          n += 8;
        }

      if ( !added_subs )
          break;

      next.swap (cur);
      next.clear();
      level++;
    }

  printf ("Done, added %u\n", (unsigned int)n);
  #endif
}

void OctreeBuilder::rebuild (Octree& tree)
{
  if ( cur.empty() || tree.levels.empty() )
    {
      build (tree);
      return;
    }

  timer t;

  vect3d box = tree.levels.back().bs;

  TaskManager tmgr;
  std::vector<fieldtask> ftasks;
  ftasks.resize (THREADS_CPUCOUNT);
  for ( size_t i = 0; i < ftasks.size(); i++ )
    {
      ftasks[i].part = i;
      ftasks[i].builder = this;
      ftasks[i].values_map_calc = &values_map_calc;
      tmgr.addTask (&ftasks[i]);
    }

  while ( tmgr.run() );
  t.report ("cornercalc");

  std::vector<celltask> ctasks;
  ctasks.resize (THREADS_CPUCOUNT);
  for ( size_t i = 0; i < ctasks.size(); i++ )
    {
      ctasks[i].part = i;
      ctasks[i].builder = this;
      ctasks[i].cells = &cur;
      ctasks[i].box = box;
      tmgr.addTask (&ctasks[i]);
    }
  while ( tmgr.run() );
  t.report ("treecalc");
}

void OctreeBuilder::run_calc_values (std::vector<values_str*>& values_map_calc, size_t part)
{
  std::vector<CubicScalar> scalars(nScalars);
  std::vector<CubicVector> vectors(nVectors);

  for ( size_t i = 0; i < values_map_calc.size(); i++ )
    {
      if ( (i % THREADS_CPUCOUNT) != part )
          continue;
      values_str& v = *values_map_calc[i];
      const vect3d& pos = v.pos;
      v.values = new prec_t [nValues * 8];
      memset (v.values, 0, sizeof(prec_t) * nValues * 8);
      v.f = v.values;
      v.dfdx = v.values + 1 * nValues;
      v.dfdy = v.values + 2 * nValues;
      v.dfdz = v.values + 3 * nValues;
      v.d2fdxdy = v.values + 4 * nValues;
      v.d2fdxdz = v.values + 5 * nValues;
      v.d2fdydz = v.values + 6 * nValues;
      v.d3fdxdydz = v.values + 7 * nValues;

      for ( size_t j = 0; j < scalars.size(); j++ )
          scalars[j].clear();
      for ( size_t j = 0; j < vectors.size(); j++ )
          vectors[j].clear();

      addValues (pos, &scalars[0], &vectors[0]);

      for ( size_t j = 0; j < scalars.size(); j++ )
        {
          scalars[j].serialize (
              v.f + j,
              v.dfdx + j,
              v.dfdy + j,
              v.dfdz + j,
              v.d2fdxdy + j,
              v.d2fdxdz + j,
              v.d2fdydz + j,
              v.d3fdxdydz + j
          );
        }

      for ( size_t j = 0; j < vectors.size(); j++ )
        {
          vectors[j].serialize (
              v.f + nScalars + j * 3,
              v.dfdx + nScalars + j * 3,
              v.dfdy + nScalars + j * 3,
              v.dfdz + nScalars + j * 3,
              v.d2fdxdy + nScalars + j * 3,
              v.d2fdxdz + nScalars + j * 3,
              v.d2fdydz + nScalars + j * 3,
              v.d3fdxdydz + nScalars + j * 3
          );
        }

    }
}

void OctreeBuilder::run_calc_cells (std::vector<o8entry_t>& cells, size_t part, vect3d box)
{
  size_t pc = cells.size() / THREADS_CPUCOUNT;
  size_t si = part * pc;
  size_t ei = si + pc;
  if ( part == THREADS_CPUCOUNT )
      ei = cells.size();
  size_t n = ei - si;
  o8entry_t *ptr = &cells[si];

  while (n--)
    {
      o8entry_t& e = *ptr++;

      for ( size_t j = 0; j < nValues; j++ )
        {
          prec_t values[64];

          for ( size_t k = 0; k < 8; k++ )
            {
              values[0 + k] = e.vals[k]->f[j];
              values[8 + k] = e.vals[k]->dfdx[j] * box.x;
              values[16 + k] = e.vals[k]->dfdy[j] * box.y;
              values[24 + k] = e.vals[k]->dfdz[j] * box.z;
              values[32 + k] = e.vals[k]->d2fdxdy[j] * box.x * box.y;
              values[40 + k] = e.vals[k]->d2fdxdz[j] * box.x * box.z;
              values[48 + k] = e.vals[k]->d2fdydz[j] * box.y * box.z;
              values[56 + k] = e.vals[k]->d3fdxdydz[j] * box.x * box.y * box.z;
            }

          float d[64];
          for ( size_t k = 0; k < 64; k++ )
            {
              prec_t res = 0;
              for ( size_t l = 0; l < 64; l++ )
                  res += values[l] * TCC[k][l];
              d[k] = res;
            }

          // reordering for fast calculation at evaluation
          float *a = e.cell->interp->fact + j * 64;
          for ( size_t i = 0; i < 4; i++ )
              for ( size_t j = 0; j < 4; j++ )
                  for ( size_t k = 0; k < 4; k++ )
                      *a++ = d[k * 16 + j * 4 + i];
        }
    }
}

#define INSIDE_BORDER -3e-11

bool OctreeBuilder::isInside (const vect3d& c1, const vect3d& c2)
{
  for ( size_t j = 0; j < limits.size(); j++ )
    {
      vect3d& n = limits[j];
      vect3d diagonal = c2 - c1;
      if ( c1 * n < INSIDE_BORDER &&
           c2 * n < INSIDE_BORDER &&
           vect3d (c1.x + diagonal.x, c1.y, c1.z) * n < INSIDE_BORDER &&
           vect3d (c1.x, c1.y + diagonal.y, c1.z) * n < INSIDE_BORDER &&
           vect3d (c1.x, c1.y, c1.z + diagonal.z) * n < INSIDE_BORDER &&
           vect3d (c2.x - diagonal.x, c2.y, c2.z) * n < INSIDE_BORDER &&
           vect3d (c2.x, c2.y - diagonal.y, c2.z) * n < INSIDE_BORDER &&
           vect3d (c2.x, c2.y, c2.z - diagonal.z) * n < INSIDE_BORDER ) return false;
    }
  return true;
}

Octree::Octree ()
{
}

Octree::~Octree ()
{
}

void Octree::clear ()
{
  a_interp.clear();
  a_cell.clear();
  a_subcell.clear();
  a_fact.clear();
  levels.clear();
  hash.clear();
}

bool Octree::getValues (const vect3d& pos, prec_t *scalars, vect3d *vectors)
{
  if ( levels.empty() )
      return false;
  vect3d n = pos - corner;
  vect3d tp = vect3d (n.x * levels[0].rbs.x, n.y * levels[0].rbs.y, n.z * levels[0].rbs.z);
  vect3d tpf = vect3d (floor(tp.x), floor(tp.y), floor(tp.z));
  size_t xi = prec2size_t(tpf.x);
  size_t yi = prec2size_t(tpf.y);
  size_t zi = prec2size_t(tpf.z);
  vect3d p = tp - tpf;
  size_t index = zi + yi * zc + xi * yc * zc;
  cell_t *cell = hash[index % hash.size()];
  while ( cell && cell->index != index )
      cell = cell->next;
  if ( !cell || !cell->interp )
      return false;

  // tricubic interpolation parameters
  prec_t f[64];
  f[0] = 1; // 1
  f[1] = p.z; // p.z
  f[2] = f[1]*p.z; // p.z*p.z
  f[3] = f[2]*p.z; // p.z*p.z*p.z
  f[4] = p.y; // p.y
  f[5] = p.y*p.z; // p.y*p.z
  f[6] = f[5]*p.z; // p.y*p.z*p.z
  f[7] = f[6]*p.z; // p.y*p.z*p.z*p.z
  f[8] = p.y*p.y; // p.y*p.y
  f[9] = f[8]*p.z; // p.y*p.y*p.z
  f[10] = f[9]*p.z; // p.y*p.y*p.z*p.z
  f[11] = f[10]*p.z; // p.y*p.y*p.z*p.z*p.z
  f[12] = f[8]*p.y; // p.y*p.y*p.y
  f[13] = f[12]*p.z; // p.y*p.y*p.y*p.z
  f[14] = f[13]*p.z; // p.y*p.y*p.y*p.z*p.z
  f[15] = f[14]*p.z; // p.y*p.y*p.y*p.z*p.z*p.z
  f[16] = p.x; // p.x
  f[17] = f[16]*p.z; // p.x*p.z
  f[18] = f[17]*p.z; // p.x*p.z*p.z
  f[19] = f[18]*p.z; // p.x*p.z*p.z*p.z
  f[20] = f[16]*p.y; // p.x*p.y
  f[21] = f[20]*p.z; // p.x*p.y*p.z
  f[22] = f[21]*p.z; // p.x*p.y*p.z*p.z
  f[23] = f[22]*p.z; // p.x*p.y*p.z*p.z*p.z
  f[24] = f[20]*p.y; // p.x*p.y*p.y
  f[25] = f[24]*p.z; // p.x*p.y*p.y*p.z
  f[26] = f[25]*p.z; // p.x*p.y*p.y*p.z*p.z
  f[27] = f[26]*p.z; // p.x*p.y*p.y*p.z*p.z*p.z
  f[28] = f[24]*p.y; // p.x*p.y*p.y*p.y
  f[29] = f[28]*p.z; // p.x*p.y*p.y*p.y*p.z
  f[30] = f[29]*p.z; // p.x*p.y*p.y*p.y*p.z*p.z
  f[31] = f[30]*p.z; // p.x*p.y*p.y*p.y*p.z*p.z*p.z
  f[32] = p.x*p.x; // p.x*p.x
  f[33] = f[32]*p.z; // p.x*p.x*p.z
  f[34] = f[33]*p.z; // p.x*p.x*p.z*p.z
  f[35] = f[34]*p.z; // p.x*p.x*p.z*p.z*p.z
  f[36] = f[32]*p.y; // p.x*p.x*p.y
  f[37] = f[36]*p.z; // p.x*p.x*p.y*p.z
  f[38] = f[37]*p.z; // p.x*p.x*p.y*p.z*p.z
  f[39] = f[38]*p.z; // p.x*p.x*p.y*p.z*p.z*p.z
  f[40] = f[36]*p.y; // p.x*p.x*p.y*p.y
  f[41] = f[40]*p.z; // p.x*p.x*p.y*p.y*p.z
  f[42] = f[41]*p.z; // p.x*p.x*p.y*p.y*p.z*p.z
  f[43] = f[42]*p.z; // p.x*p.x*p.y*p.y*p.z*p.z*p.z
  f[44] = f[40]*p.y; // p.x*p.x*p.y*p.y*p.y
  f[45] = f[44]*p.z; // p.x*p.x*p.y*p.y*p.y*p.z
  f[46] = f[45]*p.z; // p.x*p.x*p.y*p.y*p.y*p.z*p.z
  f[47] = f[46]*p.z; // p.x*p.x*p.y*p.y*p.y*p.z*p.z*p.z
  f[48] = f[32]*p.x; // p.x*p.x*p.x
  f[49] = f[48]*p.z; // p.x*p.x*p.x*p.z
  f[50] = f[49]*p.z; // p.x*p.x*p.x*p.z*p.z
  f[51] = f[50]*p.z; // p.x*p.x*p.x*p.z*p.z*p.z
  f[52] = f[48]*p.y; // p.x*p.x*p.x*p.y
  f[53] = f[52]*p.z; // p.x*p.x*p.x*p.y*p.z
  f[54] = f[53]*p.z; // p.x*p.x*p.x*p.y*p.z*p.z
  f[55] = f[54]*p.z; // p.x*p.x*p.x*p.y*p.z*p.z*p.z
  f[56] = f[52]*p.y; // p.x*p.x*p.x*p.y*p.y
  f[57] = f[56]*p.z; // p.x*p.x*p.x*p.y*p.y*p.z
  f[58] = f[57]*p.z; // p.x*p.x*p.x*p.y*p.y*p.z*p.z
  f[59] = f[58]*p.z; // p.x*p.x*p.x*p.y*p.y*p.z*p.z*p.z
  f[60] = f[56]*p.y; // p.x*p.x*p.x*p.y*p.y*p.y
  f[61] = f[60]*p.z; // p.x*p.x*p.x*p.y*p.y*p.y*p.z
  f[62] = f[61]*p.z; // p.x*p.x*p.x*p.y*p.y*p.y*p.z*p.z
  f[63] = f[62]*p.z; // p.x*p.x*p.x*p.y*p.y*p.y*p.z*p.z*p.z

  float *fact = cell->interp->fact;
  if ( scalars )
    {
      for ( size_t i = 0; i < nScalars; i++ )
        {
          prec_t res = 0;
          prec_t *ff = f;
          for ( size_t j = 0; j < 64; j++ )
              res += *fact++ * *ff++;
          scalars[i] = res;
        }
    }
  else
    {
      fact += nScalars * 64;
    }

  if ( vectors )
    {
      for ( size_t i = 0; i < nVectors; i++ )
        {
          prec_t *o = (prec_t *)&vectors[i];
          for ( size_t n = 0; n < 3; n++ )
            {
              prec_t res = 0;
              prec_t *ff = f;
              for ( size_t j = 0; j < 64; j++ )
                  res += *fact++ * *ff++;
              o[n] = res;
            }
        }
    }

  return true;
}

std::string Octree::getPovrayScript()
{
  std::string ret;
  for ( size_t i = 0; i < hash.size(); i++ )
    {
      cell_t *cell = hash[i];
      while ( cell )
        {
          addPovrayScript (ret, cell, 0);
          cell = cell->next;
        }
    }
  return ret;
}

void Octree::addPovrayScript (std::string& str, cell_t *cell, size_t level)
{
  if ( cell->subcells )
    {
      for ( size_t i = 0; i < 8; i++ )
          addPovrayScript (str, cell->subcells->sc[i], level + 1);
      return;
    }

  vect3d c1;
  if ( cell->interp )
      c1 = cell->interp->corner;
  else
      assert(0);

  vect3d c2 = c1 + levels[level].bs;

  char buf[1024];
  sprintf (buf, "box {<%.4f, %.4f, %.4f>, <%.4f, %.4f, %.4f> texture {pigment {rgb <%.2f, %.2f, %.2f>}}}\n",
      prec2double (c1.x), prec2double (c1.y), prec2double (c1.z),
      prec2double (c2.x), prec2double (c2.y), prec2double (c2.z),
      prec2double (prec_t_drand()), prec2double (prec_t_drand()), prec2double (prec_t_drand()));
  str += buf;
}

size_t Octree::size () const
{
  //printf ("%zu %zu %zu %zu\n", a_interp.size(), a_cell.size(), a_subcell.size(), a_fact.size());
  return a_interp.size() + a_cell.size() + a_subcell.size() + a_fact.size();
}

#define OCTREE_MAGIK "OcT1"

bool Octree::save (const char *path)
{
  FILE *fp = fopen (path, "wb");
  if ( !fp )
      return false;
  FPWriter w(fp);
  w.write (OCTREE_MAGIK, 4);
  w.write (corner);
  w.write (box);
  w.write (hbox);
  w.write (rbox);
  w.write (xc);
  w.write (yc);
  w.write (zc);

  w.write (nScalars);
  w.write (nVectors);
  w.write (nValues);

  w.write (levels.size());
  for ( size_t i = 0; i < levels.size(); i++ )
    {
      w.write (levels[i].bs);
      w.write (levels[i].hbs);
      w.write (levels[i].rbs);
    }

  size_t nCells = 0, nInterp = 0, nSubcell = 0;
  cmap_t cmap;
  size_t toplevel = 0;
  for ( size_t i = 0; i < hash.size(); i++ )
    {
      cell_t *cell = hash[i];
      while ( cell )
        {
          toplevel++;
          get_stats (cell, nCells, nInterp, nSubcell, cmap);
          cell = cell->next;
        }
    }

  w.write (nCells);
  w.write (nInterp);
  w.write (nSubcell);
  w.write (toplevel);
  for ( size_t i = 0; i < hash.size(); i++ )
    {
      cell_t *cell = hash[i];
      while ( cell )
        {
          save (w, cmap, cell);
          cell = cell->next;
        }
    }
  return true;
}

void Octree::save (FPWriter& w, cmap_t& cmap, cell_t *cell)
{
  w.write (cell->index);
  if ( cell->interp )
    {
      w.write_char (1);
      w.write (cell->interp->corner);
      w.write (cell->interp->fact, sizeof (float) * 64 * nValues);
    }
  else
    {
      w.write_char (2);
      w.write (cell->subcells->middle);
      for ( size_t i = 0; i < 8; i++ )
          w.write (cmap[cell->subcells->sc[i]]);
      for ( size_t i = 0; i < 8; i++ )
          save (w, cmap, cell->subcells->sc[i]);
    }
}

bool Octree::load (const char *path)
{
  clear();
  FILE *fp = fopen (path, "rb");
  if ( !fp )
      return false;
  FPReader r(fp);
  char m[4];
  r.read (m, 4);
  if ( memcmp (m, OCTREE_MAGIK, 4) != 0 )
      return false;
  r.read (corner);
  r.read (box);
  r.read (hbox);
  r.read (rbox);
  r.read (xc);
  r.read (yc);
  r.read (zc);
  r.read (nScalars);
  r.read (nVectors);
  r.read (nValues);
  size_t lc;
  r.read (lc);
  levels.resize (lc);
  for ( size_t i = 0; i < levels.size(); i++ )
    {
      r.read (levels[i].bs);
      r.read (levels[i].hbs);
      r.read (levels[i].rbs);
    }
  size_t nCells, nInterp, nSubcell, nToplevel;
  r.read (nCells);
  r.read (nInterp);
  r.read (nSubcell);
  r.read (nToplevel);

  hash.resize (3 * nToplevel / 2 + 17 - nToplevel % 17, 0);

  a_cell.reserve (nCells);
  a_interp.reserve (nInterp);
  a_subcell.reserve (nSubcell);
  a_fact.reserve (nValues * 64 * nInterp);
  cell_t *cells = a_cell.get (nCells);
  for ( size_t i = 0; i < nCells; i++ )
    {
      cell_t& cell = cells[i];
      memset (&cell, 0, sizeof (cell));
      r.read (cell.index);
      if ( cell.index != (size_t)-1 )
        {
          size_t hi = cell.index % hash.size();
          cell.next = hash[hi];
          hash[hi] = &cell;
        }
      char t;
      r.read_char (t);
      if ( t == 1 )
        {
          cell.interp = a_interp.get();
          r.read (cell.interp->corner);
          cell.interp->fact = a_fact.get (64 * nValues);
          r.read (cell.interp->fact, sizeof (float) * 64 * nValues);
        }
      else if ( t == 2 )
        {
          cell.subcells = a_subcell.get();
          r.read (cell.subcells->middle);
          for ( size_t i = 0; i < 8; i++ )
            {
              size_t ci;
              r.read (ci);
              cell.subcells->sc[i] = &cells[ci];
            }

        }
      else
        {
          return false;
        }
    }
  return true;
}

void Octree::get_stats (cell_t *cell, size_t& nCells, size_t& nInterp, size_t& nSubcell,
    std::map<void *, size_t>& cmap)
{
  cmap[cell] = nCells;
  nCells++;

  if ( cell->interp )
    {
      nInterp++;
    }
  else
    {
      nSubcell++;
      for ( size_t i = 0; i < 8; i++ )
          get_stats (cell->subcells->sc[i], nCells, nInterp, nSubcell, cmap);
    }
}

